import numpy as np
import cv2

def cal_radius(img,left_fit,right_fit):

    # 图像中像素个数与实际中距离的比率
    # 沿车行进的方向长度大概覆盖了30米，按照中国公路的标准，宽度为3.7米(经验值)
    ym_per_pix = 30 / 720
    xm_per_pix = 3.7 / 640

    # 计算得到曲线上的每个点
    left_y_axis = np.linspace(0,img.shape[0],img.shape[0] - 1)
    left_x_axis = left_fit[0] * left_y_axis**2 + left_fit[1] * left_y_axis + left_fit[2]
    right_y_axis = np.linspace(0, img.shape[0], img.shape[0] - 1)
    right_x_axis = right_fit[0] * right_y_axis ** 2 + right_fit[1] * right_y_axis + right_fit[2]

    # 获取真实环境中的曲线
    left_fit_cr = np.polyfit(left_y_axis * ym_per_pix,left_x_axis * xm_per_pix,2)
    right_fit_cr = np.polyfit(right_y_axis * ym_per_pix,right_x_axis * xm_per_pix,2)

    # 获得真实环境中的曲率
    left_curverad = ((1 + (2 * left_fit_cr[0] * left_y_axis * ym_per_pix + left_fit_cr[1]) ** 2) ** 1.5) / np.absolute(2 * left_fit_cr[0])

    right_curverad = ((1 + (2 * right_fit_cr[0] * right_y_axis * ym_per_pix + right_fit_cr[1]) ** 2) ** 1.5) / np.absolute(2 * right_fit_cr[0])

    # 在图像上显示曲率
    cv2.putText(img,'Radius of Curvature = {}(m)'.format(np.mean(left_curverad)),(20,50),0,0.8,color=(255,255,255),thickness=2)
    return img